#include <string.h>
#include <stdio.h>
#include <math.h>
#include <syslog.h>
#include <stdlib.h>
#include "phylip.h"
#include "draw.h"
#include "drawtree.h"

#define GAP 0.5
#define MAXITERATIONS 100
#define MINIMUMCHANGE   0.0001

/** added - danieyek 990208 
 * Find the median of the distance; used to compute the angle to rotate
 * in proportion to the size of the graph and forces.
 * It is assumed that pRootSubNode is also the pivot (during the first 
 * call to this function) - the center, with
 * respect to which node the distances are calculated.
 * If there are only 3 element, element #2 is returned, ie. (2+1)/2.
 * This function now finds the median of distances of all nodes, not only
 * the leafs! 
 */
double medianOfDistance(node *pRootSubNode, boolean firstRecursiveCallP)
{
  	node *pSubNode;
  	double xDelta, yDelta, distance;
  	long i, j;
  	struct dblLinkNode
  	{
    	double value;
    	/* Implement reverse Linked List */
    	struct dblLinkNode *pBack;
  	} *pLink, *pBackElem, *pMidElem, *pFrontElem, junkLink;
  	/* must use static to retain values over calls */
  	static node *pReferenceNode;
  	static long count;
  	static struct dblLinkNode *pFrontOfLinkedList;

  	/* Remember the reference node so that it doesn't have to be passed 
     around in the function parameter. */
  	if (firstRecursiveCallP == true)
  	{
    	pReferenceNode = pRootSubNode;
    	pFrontOfLinkedList = NULL;
   	 	count = 0;
  	}
  	pSubNode = pRootSubNode;
  	/* visit the rest of the branches of current node; the branch attaches to 
     the current subNode may be visited in the code further down below. */
  	while (pSubNode->next != NULL && pSubNode->next != pRootSubNode)
  	{
    	pSubNode = pSubNode->next;
    	if ( pSubNode->back != NULL) 
      		medianOfDistance(pSubNode->back, false);
  	}
  	/* visit this branch; You need to visit it for the first time - at root 
     only! use pRootSubNode instead of pSubNode here because pSubNode stop 
     short just before pRootSubNode (the entry node) */
  	if ( firstRecursiveCallP == true && pRootSubNode->back != NULL)
    	medianOfDistance(pRootSubNode->back, false);
  	/* Why only leafs count?  Modifying it!  */
  	xDelta = nodep[pSubNode->index-1]->xcoord - 
    	nodep[pReferenceNode->index-1]->xcoord;
  	yDelta = nodep[pSubNode->index-1]->ycoord - 
    	nodep[pReferenceNode->index-1]->ycoord;
  	distance = sqrt( xDelta*xDelta + yDelta*yDelta );
  	/* Similar to pushing onto the stack */
  	pLink = (struct dblLinkNode*) Malloc( sizeof(struct dblLinkNode) );
  	if (pLink == NULL)
  	{
    	syslog(LOG_ERR,"Fatal ERROR: drawtree - Insufficient Memory in" 
        " medianOfDistance()!\n");
    	exit(-1);
  	}
  	pLink->value = distance;
  	pLink->pBack = pFrontOfLinkedList;
  	pFrontOfLinkedList = pLink;
  	count = count + 1;
  	if (firstRecursiveCallP == true) 
  	{
    	if (count == 0)
    	{
      		return (double)0;
    	}
    	else if (count == 1)
    	{
      		distance = pFrontOfLinkedList->value;
      		free(pFrontOfLinkedList);
      		return distance;
    	}
    	else if (count == 2)
    	{
      		distance = (pFrontOfLinkedList->value + 
          	pFrontOfLinkedList->pBack->value)/(double)2;
      		free(pFrontOfLinkedList->pBack);
      		free(pFrontOfLinkedList);
      		return distance;
    	}
    	else
    	{
      		junkLink.pBack = pFrontOfLinkedList;
	
      		/* SORT first - use bubble sort; we start with at least 3 elements here. */
      		/* We are matching backward when sorting the list and comparing MidElem and 
         	BackElem along the path;  junkLink is there just to make a symmetric
         	operation at the front end. */
      		for (j = 0; j < count - 1; j++)
      		{
            	pFrontElem = &junkLink;
            	pMidElem = junkLink.pBack;
            	pBackElem = junkLink.pBack->pBack;
        		for (i = j; i < count - 1; i++)
        		{
          			if(pMidElem->value < pBackElem->value)
          			{	
          				/* Swap - carry the smaller value to the root of the linked list. */
            			pMidElem->pBack = pBackElem->pBack;
            			pBackElem->pBack = pMidElem;
            			pFrontElem->pBack = pBackElem;
            			/* Correct the order of pFrontElem, pMidElem, pBackElem and match 
               			one step */
            			pFrontElem = pBackElem;
            			pBackElem = pMidElem->pBack;
          			}
          			else
          			{
            			pFrontElem = pMidElem;
            			pMidElem = pBackElem;
            			pBackElem = pBackElem->pBack;
          			}
        		}
        		pFrontOfLinkedList = junkLink.pBack;
      		}
      		/* Sorted; now get the middle element. */
      		for (i = 1; i < (count + 1)/(long) 2; i++)
      		{
       		 	/* Similar to Poping the stack */
              	pLink = pFrontOfLinkedList;
        		pFrontOfLinkedList = pLink->pBack;
        		free(pLink);
      		}
    		/* Get the return value!! - only the last return value is the valid one. */
      		distance = pFrontOfLinkedList->value;

      		/* Continue from the same i value left off by the previous for loop!  */
      		for (; i <= count; i++)
      		{
        		/* Similar to Poping the stack */
              	pLink = pFrontOfLinkedList;
        		pFrontOfLinkedList = pLink->pBack;
        		free(pLink);
      		}
    	}
  	}
  	return distance;
}
double dotProduct(double Xu, double Yu, double Xv, double Yv)
{
  	return Xu * Xv + Yu * Yv;
} 
/* Calculate angle between 2 vectors; value returned is always between
   0 and pi.  
   Use vCounterClkwiseU() to get the relative position of the vectors.  */ 
double angleBetVectors(double Xu, double Yu, double Xv, double Yv)
{
  	double dotProd, cosTheta, theta, lengthsProd;
  	dotProd = dotProduct(Xu, Yu, Xv, Yv);
  	lengthsProd = sqrt(Xu*Xu+Yu*Yu) * sqrt(Xv*Xv+Yv*Yv);

  	if (lengthsProd < epsilon)
  	{
    	syslog(LOG_ERR,"drawtree - division by zero in angleBetVectors()!\n"
    	"Xu %f Yu %f Xv %f Yv %f\n", Xu, Yu, Xv, Yv);
    	exit(-1);
  	}
  	cosTheta = dotProd / lengthsProd;

  	if (cosTheta > 1) /* cosTheta will only be > 1 or < -1 due to rounding errors */
    	theta = 0; /* cosTheta = 1 */
  	else if (cosTheta < -1)
    	theta = pi; /* cosTheta = -1 */
  	else 
    	theta = acos(cosTheta);
  	return theta;
}
/* As usual, pToSubNode->back is the angle 
   leftLimit is the max angle you can rotate on the left and 
   rightLimit vice versa.
   *pLeftLimit and *pRightLimit must be initialized to 0; without
   initialization, it would introduce bitter bugs into the program;
   they are initialized in this routine.
*/
void leftRightLimits(node *pToSubNode, double *pLeftLimit, 
	double *pRightLimit)
{
  	/* pPivotNode is nodep[pToSubNode->back->index-1], not pPivotSubNode
     which is just pToSubNode->back! */
  	node *pLeftSubNode, *pRightSubNode, *pPivotNode, *pSubNode;
  	double leftLimit, rightLimit, xToNodeVector, yToNodeVector, xLeftVector, 
    	yLeftVector, xRightVector, yRightVector, lengthsProd;

  	*pLeftLimit = 0;
  	*pRightLimit = 0;
  	/* Make an assumption first - guess "pToSubNode->back->next" is the right 
     and the opposite direction is the left! */
  	/* It shouldn't be pivoted at a left, but just checking. */
  	if (pToSubNode->back->tip == true)
  	{
   		/* Logically this should not happen.  But we actually can return pi 
      	as the limit. */
    	syslog(LOG_ERR,"In leftRightLimits() - Pivoted at a leaf! Unable to " 
        	"calculate left and right limit.\n");
    	exit(-1);
  	}
  	else if (pToSubNode->back->next->next == pToSubNode->back)
  	{
    	*pLeftLimit = 0; /* don't pivot where there is no branching */
    	*pRightLimit = 0;
    	return;
  	}
  	/* Else, do this */
  	pPivotNode = nodep[pToSubNode->back->index-1];

  	/* 3 or more branches - the regular case. */
  	/* First, initialize the pRightSubNode - non-repeative portion of the code */
  	pRightSubNode = pToSubNode->back;
  	pLeftSubNode = pToSubNode->back;
  	xToNodeVector = nodep[pToSubNode->index-1]->xcoord - pPivotNode->xcoord;
  	yToNodeVector = nodep[pToSubNode->index-1]->ycoord - pPivotNode->ycoord;

  	/* If both x and y are 0, then the length must be 0; but this check is not
     enough yet, we need to check the product of length also. */
  	if ( fabs(xToNodeVector) < epsilon && fabs(yToNodeVector) < epsilon )
  	{
    	/* If the branch to rotate is too short, don't rotate it. */
    	*pLeftLimit = 0;
    	*pRightLimit = 0;
    	return;
  	}
  	while( nodep[pRightSubNode->index-1]->tip != true )
  	{
    	/* Repeative code */
    	pRightSubNode = pRightSubNode->next->back;

    	xRightVector = nodep[pRightSubNode->index-1]->xcoord - pPivotNode->xcoord;
    	yRightVector = nodep[pRightSubNode->index-1]->ycoord - pPivotNode->ycoord;

    	lengthsProd = sqrt(xToNodeVector*xToNodeVector+yToNodeVector*yToNodeVector)
    	  * sqrt(xRightVector*xRightVector+yRightVector*yRightVector);
   	 	if ( lengthsProd < epsilon )
    	{
      		continue;
    	}
    	rightLimit = angleBetVectors(xToNodeVector, yToNodeVector, xRightVector, 
        	yRightVector);
    	if ( (*pRightLimit) < rightLimit) *pRightLimit = rightLimit;
  	}

  	while( nodep[pLeftSubNode->index-1]->tip != true )
  	{
    	/* First, let pSubNode be 1 subnode after rightSubNode. */
    	pSubNode = pLeftSubNode->next->next;
    	/* Then, loop until the last subNode before getting back to the pivot */
    	while (pSubNode->next != pLeftSubNode)
    	{
      		pSubNode = pSubNode->next;
    	}
    	pLeftSubNode = pSubNode->back; 
    	xLeftVector = nodep[pLeftSubNode->index-1]->xcoord - pPivotNode->xcoord;
    	yLeftVector = nodep[pLeftSubNode->index-1]->ycoord - pPivotNode->ycoord;

    	lengthsProd = sqrt(xToNodeVector*xToNodeVector+yToNodeVector*yToNodeVector)
      	* sqrt(xLeftVector*xLeftVector+yLeftVector*yLeftVector);
    	if ( lengthsProd < epsilon )
    	{
      		continue;
    	}
    	leftLimit = angleBetVectors(xToNodeVector, yToNodeVector, xLeftVector, 
        	yLeftVector);

    	if ( (*pLeftLimit) < leftLimit) *pLeftLimit = leftLimit;
  	}
  	return;
}
/* compute the angle of a vector */
double angleof(double x, double y)
{
  	double theta;

  	if (fabs(x) > epsilon)
    	theta = atan(y / x);
  	else if (y >= 0.0)
      	theta = pi / 2;
    else
      	theta = 1.5 * pi;
  	if (x < -epsilon)
    	theta = pi + theta;
  	while (theta > 2 * pi)
    	theta -= 2 * pi;
  	while (theta < 0.0)
    	theta += 2 * pi;
  	return theta;
}
/* get leftmost and rightmost angle of subtree, put them in node p */
void leftrightangle(node *p, double xx, double yy)
{
  	double firstx, firsty, leftx, lefty, rightx, righty;
  	double langle, rangle;

  	firstx = nodep[p->back->index-1]->xcoord - xx;
  	firsty = nodep[p->back->index-1]->ycoord - yy;
  	leftx = firstx;
  	lefty = firsty;
  	rightx = firstx;
  	righty = firsty;
  	if (p->back != NULL)
    	polartrav(p->back,xx,yy,firstx,firsty,&leftx,&lefty,&rightx,&righty);
  	if ((fabs(leftx) < epsilon) && (fabs(lefty) < epsilon))
    	langle = p->back->oldtheta;
  	else
    	langle = angleof(leftx, lefty);
  	if ((fabs(rightx) < epsilon) && (fabs(righty) < epsilon))
    	rangle = p->back->oldtheta;
  	else
    	rangle = angleof(rightx, righty);
  	while (langle - rangle > 2*pi)
    	langle -= 2 * pi;
  	while (rangle > langle) 
	{
    	if (rangle > 2*pi)
      		rangle -= 2 * pi;
    	else
      		langle += 2 * pi;
  	}
  	while (langle > 2*pi) 
	{
    	rangle -= 2 * pi;
    	langle -= 2 * pi;
  	}
  	p->lefttheta = langle;
  	p->righttheta = rangle;
}
/** 
 * Read comment for totalForceOnNode
 * It supposed to return a positive value to indicate that it has a positive 
 * moment; and negative return value to indicate negative moment. 
 * force perpendicular at norminal distance 1 is taken to be 1.  
 * medianDistance is the median of Distances in this graph. 
 *
                        / Force
                       /  
          |  ToNode   o  > alpha
          |              \  
   yDelta |               \     theta = pi/2 + alpha
          |                  beta = vector (or angle) from Pivot to ToNode
  Pivot   o-----------
             xDelta

  alpha = theta + beta
 */
double forcePerpendicularOnNode(node *pPivotSubNode, node *pToSubNode,
	double medianDistance)
{
  	double totalForce, forceAngle, xDelta, yDelta;
  	double alpha, theta, forcePerpendicular, sinForceAngle, cosForceAngle;

  	totalForce = (double)0;
  	forceAngle = (double)0;
  	totalForceOnNode(pPivotSubNode, pToSubNode, &totalForce, &forceAngle, 
      	medianDistance);
  	xDelta = nodep[pToSubNode->index-1]->xcoord - 
    	nodep[pPivotSubNode->index-1]->xcoord;
  	yDelta = nodep[pToSubNode->index-1]->ycoord - 
    nodep[pPivotSubNode->index-1]->ycoord;
  	/* Try to avoid the case where 2 nodes are on top of each other. */
  	/*
  	if (xDelta < 0) tempx = -xDelta;
  	else tempx = xDelta;
  	if (yDelta < 0) tempy = -yDelta;
  	else tempy = yDelta;
  	if (tempx < epsilon && tempy < epsilon)
 	{
    	return;
  	}
  	*/
  	sinForceAngle = sin(forceAngle);
  	cosForceAngle = cos(forceAngle);
  	theta = angleBetVectors(xDelta, yDelta, cosForceAngle, sinForceAngle);
  	if (theta > pi/2)
  	{
    	alpha = theta - pi/2;
  	}
  	else
  	{
    	alpha = pi/2 - theta;
  	}
  	forcePerpendicular = totalForce * cos(alpha);

  	if (forcePerpendicular < -epsilon)
  	{
    	syslog(LOG_ERR,"drawtree - forcePerpendicular applied at an angle should" 
        " not be less than zero (in forcePerpendicularOnNode()). alpha = %f\n", alpha);
    	exit(-1);
  	}
  	/* correct the sign of the moment */
  	forcePerpendicular = signOfMoment(xDelta, yDelta, cosForceAngle, 
      	sinForceAngle) 
    	* forcePerpendicular;
  	return forcePerpendicular;
}
